#include<stdio.h>
#include<Mesh/libcell_iterator.h>
#include<Mesh_IO/Mesh_IO.h>

#include<tool/libcell_octree_iterator.h>
#include<tool/libcell_tools_octree.h>
#include<Algorithm/points_cloud_algorithm.h>
#include<Algorithm/poisson_construct.h>
#include <Arcroll.h>
#include<Algorithm/marching_cubes.h>
#include<libcell_macros.h>
#include<IterativeLinearSolvers>
#include<Sparse>
//#include<vector>
using namespace Eigen;

extern double fx_dot_fx[5];
extern double fx_dot_Dfx[5];
extern double fx_dot_DDfx[5];


static void  test_show_grid_normal(Viewer_World*vw,Poisson_Grid*pg)
{ 
    Node*n2=vw->create_something(vw,"Edges");
    Viewer_Something* vs=(Viewer_Something*)(n2->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    int sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    sum++;
                }
            }
        }
    } 
    ve->Data_rows=sum*2;
    ve->Data_index_rows=sum;
    ve->color_rows=ve->Data_index_rows;
    ve->Data=(float *)malloc(sizeof(float)*3*ve->Data_rows);    
    ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    ve->set_color(ve,0,1.0,0.0,1.0);  
    sum=0;
    //printf("data rows:%d index rows:%d\n",ve->Data_rows,ve->Data_index_rows);
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    ve->Data[(sum*2)*3+0]=pg->pgps[i][j][k].pos[0];
                    ve->Data[(sum*2)*3+1]=pg->pgps[i][j][k].pos[1];
                    ve->Data[(sum*2)*3+2]=pg->pgps[i][j][k].pos[2];
                    
                    ve->Data[(sum*2+1)*3+0]= ve->Data[(sum*2)*3+0] +pg->pgps[i][j][k].normal_coeff[0]*0.1;
                    ve->Data[(sum*2+1)*3+1]= ve->Data[(sum*2)*3+1] +pg->pgps[i][j][k].normal_coeff[1]*0.1;
                    ve->Data[(sum*2+1)*3+2]= ve->Data[(sum*2)*3+2] +pg->pgps[i][j][k].normal_coeff[2]*0.1;
                    
                    //printf("%lf %lf %lf %lf %lf %lf\n",pg->pgps[i][j][k].pos[0],pg->pgps[i][j][k].pos[1],pg->pgps[i][j][k].pos[2],pg->pgps[i][j][k].normal_coeff[0],pg->pgps[i][j][k].normal_coeff[1],pg->pgps[i][j][k].normal_coeff[2]);
                    ve->Data_index[sum*2]=sum*2;
                    ve->Data_index[sum*2+1]=sum*2+1; 
                    sum++;
                    //vp->Data[sum*3+0]=pg->pgps ; 
                }
            }
        }
    }  
    free_node(n2);
}
static void test_show_triangles(Viewer_World* vw, Node* node_tri)
{
    Node* n=vw->create_something(vw,"Faces");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Faces* vf=(Viewer_Faces*)(vs->evolution);
     
    vf->Data_rows=node_size(node_tri)*3;
    vf->Data_index_rows=node_size(node_tri);
    printf("node_tr size:%d\n",vf->Data_rows);

    vf->Data=(float*)malloc(sizeof(float)*3*vf->Data_rows);
    vf->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*4*vf->Data_index_rows);
    vf->normal_rows=vf->Data_index_rows;
    vf->normal=(float*)malloc(sizeof(float)*3*vf->normal_rows);
    memset(vf->normal,0,sizeof(float)*3*vf->normal_rows);
    int i=0;
    for(Node* nit=node_tri;nit!=NULL;nit=(Node*)(nit->Next))
    {
        TRIANGLE* triangle=(TRIANGLE*)(nit->value);
        normalize(triangle->normals[0],3);
        for(int j=0;j<3;j++)
        {
            for(int k=0;k<3;k++)
            {
                vf->Data[i*9+j*3+k]=triangle->p[j][k];  
            }
            vf->normal[i*3+j]=triangle->normals[0][j];
        }
        vf->Data_index[i*4+0]=3; vf->Data_index[i*4+1]=i*3+0;vf->Data_index[i*4+2]=i*3+1;vf->Data_index[i*4+3]=i*3+2;
        i++;
    } 
    vf->color_rows=vf->Data_index_rows;
    vf->set_color(vf,0.9,0.6,0.7,1.0);
    free_node(n); 
}
void fill_normal_coeff(Poisson_Grid*pg,Node* node_vertices)
{
    int i=0,j=0,k=0;
    double* normal=NULL;
    for(Node* nit=node_vertices;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        i=(int)((v->point[0]+pg->range)/pg->width);
        j=(int)((v->point[1]+pg->range)/pg->width);
        k=(int)((v->point[2]+pg->range)/pg->width);
        i=(i<0?0:(i>=pg->size? (pg->size-1): i ));
        j=(j<0?0:(j>=pg->size? (pg->size-1): j )); 
        k=(k<0?0:(k>=pg->size? (pg->size-1): k ));
        if(pg->pgps[i][j][k].normal_coeff[0]==0&&pg->pgps[i][j][k].normal_coeff[1]==0&&pg->pgps[i][j][k].normal_coeff[2]==0)
        {
            normal=(double*)(v->prop);
            pg->pgps[i][j][k].normal_coeff[0]=normal[0];
            pg->pgps[i][j][k].normal_coeff[1]=normal[1];
            pg->pgps[i][j][k].normal_coeff[2]=normal[2];
        } 
    }
}
Viewer_Something* test_show_grid_points(Viewer_World* vw,Poisson_Grid* pg)
{
    printf("bbegin show grid\n");
    Node* n=vw->create_something(vw,"Points");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Points* vp=(Viewer_Points*)(vs->evolution);
    int sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    sum++;
                }
            }
        }
    }
    vp->Data_rows=sum;
    vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
    vp->set_color(vp,1,0,0,1.0);

    printf("sum%d\n",sum);
    sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    vp->Data[sum*3+0]=pg->pgps[i][j][k].pos[0];
                    vp->Data[sum*3+1]=pg->pgps[i][j][k].pos[1];
                    vp->Data[sum*3+2]=pg->pgps[i][j][k].pos[2];
                    sum++;
                }
            }
        }
    } 

    vp->pointsize=6.0;
    free_node(n);
    return vs;
}

void  poisson_test_sparse_matrix(Poisson_Grid*pg )
{
    int len=pg->size*pg->size*pg->size;
    SparseMatrix<double, RowMajor> A(len,len);
    A.reserve(VectorXi::Constant(pg->size*pg->size*pg->size,140));
    printf("begin sparse matrix %d \n",pg->size);

    int id=0,id1=0;
    int sum=0;double temp=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                id=threeD_index2_id(i,j,k,pg->size);
                sum++;
                for(int l=0;l<5;l++ )
                {
                    for(int m=0;m<5;m++)
                    {
                        for(int n=0;n<5;n++)
                        {
                            if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
                            {
                                continue;
                            }
                            id1=threeD_index2_id(i+l-2,j+m-2,k+n-2,pg->size);
                            temp=0;
                            temp+=(fx_dot_DDfx[l]*fx_dot_fx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_DDfx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_DDfx[n]);
                            temp/=(pg->width*pg->width);
                            A.coeffRef(id,id1)=temp;
                            //A.insert(id,id1)=temp;
                        }
                    }
                }
            }
        }
    }
    printf("beigin compress\n");

    A.makeCompressed();

    ConjugateGradient<SparseMatrix<double>,Eigen::Upper> solver;
  
    double*bi= poisson_compute_b(pg );

    Eigen::VectorXd b(len);
    for(int i=0;i<len;i++)
    {
        b.coeffRef(i)=bi[i];
       
    }   

    printf("end b\n"); 

    free(bi);
      
    VectorXd x= solver.compute(A).solve(b); 
    pg->solve=(double*)malloc(sizeof(double)*len); 
    for(int i=0;i<len;i++)
    {
        pg->solve[i]=x.coeffRef(i);
    }
    printf(" 1sum:%d\n",sum);
}

void test()
{
    Mesh mesh;
    Mesh_init(&mesh);
    
    _ReadOff_(&mesh,"nDenseCloud.off",3);

    Node*node_vertices=NULL;
    
    for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++)
    {
        node_vertices=node_overlying(node_vertices,quote(vit));
    }

    OC_Node* ocn=(OC_Node*)malloc(sizeof(OC_Node));
    oc_node_init_from_node(ocn,node_vertices); 
    for(int i=0;i<8;i++)
    {
        int num=oc_node_divide_all_leaves(ocn);
        printf("%d\n",num);
    } 
    // int num=0;
    // for(auto oit=octree_begin(ocn);oit.it!=NULL;oit++)
    // {
    //     num++;
    // } 
    // printf("%d\n",num); 
    oc_node_free(ocn);
    Mesh_free(&mesh);
}
void test_show_vertices(Viewer_World* vw,Node*vertices)
{
    Node* n=vw->create_something(vw,"Points");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Points* vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=node_size(vertices);
    vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
    vp->set_color(vp,1,0,0,1.0);
    int i=0;
    for(Node* nit=vertices;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        vp->Data[i*3+0]=v->point[0];vp->Data[i*3+1]=v->point[1];vp->Data[i*3+2]=v->point[2];

        i++;
    } 

    free_node(n); 
}


void test_fast_compute_triangle()
{
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"rabbitCut.off",3);

    template_v ** values=(template_v**)malloc(sizeof(template_v*)*mesh.num_v(&mesh));
    int i=0;
    Node* node_vertices=NULL;
    for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++ )
    {
        values[i]=quote(vit);
        node_vertices=node_overlying(node_vertices,quote(vit));
        i++;
    }

    KD_Node*kdtree1= create_kd_tree(values,i,0); 


    free_kdnode(kdtree1);
    free_node(node_vertices);

    free(values);
}
void test1()
{
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"nDenseCloud.off",3);
 
    Node* vertices=NULL;
    for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++)
    {
        vertices=node_overlying(vertices,quote(vit));
    }
    Mesh* mesh1=downsampling_through_oc_tree(vertices,9);
    _WriteOff_(mesh1,"downsampling.off");
    Mesh_free(mesh1);
    // Node* vertices1= downsampling_through_oc_tree(vertices,8);
    // printf("size:%d\n",node_size(vertices1));
    
    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);
    Viewer_World *vw=vwm.create_world(&vwm,NULL);

    add_default_somethings(vw);  
   // test_show_vertices(vw,vertices1);
    
    Viewer_Opengl_Interpreter voi;
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);
    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm);  
    
    // free_node(vertices1);
    free_node(vertices);

    Mesh_free(&mesh);  
}
void test_poisson()
{
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"downsampling.off",3);
    mesh_adjust_mesh_to_unit_hexahedron(&mesh); 

    template_v ** values=(template_v**)malloc(sizeof(template_v*)*mesh.num_v(&mesh));
    int i=0;
    Node* node_vertices=NULL;
    for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++ )
    {
        values[i]=quote(vit);
        node_vertices=node_overlying(node_vertices,quote(vit));
        i++;
    }
    KD_Node*kdtree1= create_kd_tree(values,i,0); 
    points_cloud_compute_normal_using_clapack(node_vertices,kdtree1,8);
    double r=compute_visual_avarage_distance(node_vertices);
    adjust_normal_of_node_vertices(node_vertices,r,kdtree1);

    Poisson_Grid*pg=(Poisson_Grid*)malloc(sizeof(Poisson_Grid));
    poisson_grid_init(pg);

    poisson_grid_init_with_depth(pg,7); 

    fill_normal_coeff(pg,node_vertices);
    poisson_test_sparse_matrix(pg);
    double iso=compute_average_poisson_iso(pg);
    //Node* node_tris=marching_cubes_generate_triangles(0.2,100,my_fun2,NULL);
    Node* node_tris=marching_cubes_generate_triangles(iso,120,end_poisson_space_fun,pg);
 
    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);
    Viewer_World *vw=vwm.create_world(&vwm,NULL);

    add_default_somethings(vw);  
   // test_show_vertices(vw,vertices1);
    test_show_grid_points(vw,pg);
    // test_show_grid_normal(vw,pg); 
    test_show_triangles(vw,node_tris);
 
    Viewer_Opengl_Interpreter voi;
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);
    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm);
    poisson_grid_free(pg);
 
    free_kdnode(kdtree1);
    free_node(node_vertices);
    free(values);
    Mesh_free(&mesh);
}



int main()
{
    //test();
   //test1();
    test_poisson();

    printf("end\n");
    return 0;
}